#ifndef ROBORTS_DECISION_UNDER_ATTACK_BEHAVIOR_H
#define ROBORTS_DECISION_UNDER_ATTACK_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class UnderAttackBehavior {
 public:
  UnderAttackBehavior(ChassisExecutor* &chassis_executor,
                Blackboard* &blackboard):chassis_executor_(chassis_executor),blackboard_(blackboard){
  twist_accel.twist.linear.x=0;
  twist_accel.twist.linear.y=0;
  twist_accel.twist.linear.z=0;
  twist_accel.twist.angular.x=0;
  twist_accel.twist.angular.y=0;
  twist_accel.twist.angular.z=0;
  }

  bool Run() {
    bias = blackboard_->GetAttackBiasinfo().data;
    if(bias == 0) return false;

    auto start_odom_ = blackboard_->GetAttackedOdominfo();
    float start_yaw = tf::getYaw(start_odom_.pose.pose.orientation);
    float goal_yaw = start_yaw + bias;
    if(goal_yaw > 3.14) goal_yaw = goal_yaw - 6.28;
    else if (goal_yaw<-3.14) goal_yaw = goal_yaw + 6.28;

    current_odom_ = blackboard_->GetOdominfo();
    float current_yaw=tf::getYaw(current_odom_.pose.pose.orientation);
    float yaw_distance;
    if(current_yaw * goal_yaw >= 0) yaw_distance = fabs(current_yaw - goal_yaw);
    else {
      yaw_distance = fabs(current_yaw)+fabs(goal_yaw);
      if(yaw_distance>3.14) yaw_distance = 6.28-yaw_distance;
    }
    ROS_INFO("yaw_distance:%f",yaw_distance);

    speed = 2 * yaw_distance;

    if(bias<0) twist_accel.twist.angular.z=-speed;
    else twist_accel.twist.angular.z=speed;

    if(yaw_distance<0.2) return false;
    else {
      chassis_executor_->Execute(twist_accel);
      return true;
    }
  }

  void Cancel() {
    chassis_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }

  ~UnderAttackBehavior() = default;

 private:
  //! executor
  ChassisExecutor* const chassis_executor_;

  //! perception information
  Blackboard* const blackboard_;

  //! special twist_accel
  roborts_msgs::TwistAccel twist_accel;

  //! current odom information
  nav_msgs::Odometry current_odom_;
  float bias;
  float speed=0.0;
};
}


#endif //ROBORTS_DECISION_UNDER_ATTACK_BEHAVIOR_H
